#include "mymath.h"

const float argTab[] = {	//0-90°正弦值,每步0.5°
	0.0000, 0.0087, 0.0175, 0.0262, 0.0349, 0.0436, 0.0523, 0.0610, 0.0698, 0.0785, 0.0872, 0.0958,
	0.1045, 0.1132, 0.1219, 0.1305, 0.1392, 0.1478, 0.1564, 0.1650, 0.1736, 0.1822, 0.1908, 0.1994,
	0.2079, 0.2164, 0.2250, 0.2334, 0.2419, 0.2504, 0.2588, 0.2672, 0.2756, 0.2840, 0.2924, 0.3007,
	0.3090, 0.3173, 0.3256, 0.3338, 0.3420, 0.3502, 0.3584, 0.3665, 0.3746, 0.3827, 0.3907, 0.3987,
	0.4067, 0.4147, 0.4226, 0.4305, 0.4384, 0.4462, 0.4540, 0.4617, 0.4695, 0.4772, 0.4848, 0.4924,
	0.5000, 0.5075, 0.5150, 0.5225, 0.5299, 0.5373, 0.5446, 0.5519, 0.5592, 0.5664, 0.5736, 0.5807,
	0.5878, 0.5948, 0.6018, 0.6088, 0.6157, 0.6225, 0.6293, 0.6361, 0.6428, 0.6494, 0.6561, 0.6626,
	0.6691, 0.6756, 0.6820, 0.6884, 0.6947, 0.7009, 0.7071, 0.7133, 0.7193, 0.7254, 0.7314, 0.7373,
	0.7431, 0.7490, 0.7547, 0.7604, 0.7660, 0.7716, 0.7771, 0.7826, 0.7880, 0.7934, 0.7986, 0.8039,
	0.8090, 0.8141, 0.8192, 0.8241, 0.8290, 0.8339, 0.8387, 0.8434, 0.8480, 0.8526, 0.8572, 0.8616,
	0.8660, 0.8704, 0.8746, 0.8788, 0.8829, 0.8870, 0.8910, 0.8949, 0.8988, 0.9026, 0.9063, 0.9100,
	0.9135, 0.9171, 0.9205, 0.9239, 0.9272, 0.9304, 0.9336, 0.9367, 0.9397, 0.9426, 0.9455, 0.9483,
	0.9511, 0.9537, 0.9563, 0.9588, 0.9613, 0.9636, 0.9659, 0.9681, 0.9703, 0.9724, 0.9744, 0.9763,
	0.9781, 0.9799, 0.9816, 0.9833, 0.9848, 0.9863, 0.9877, 0.9890, 0.9903, 0.9914, 0.9925, 0.9936,
	0.9945, 0.9954, 0.9962, 0.9969, 0.9976, 0.9981, 0.9986, 0.9990, 0.9994, 0.9997, 0.9998, 0.9999, 1.0000
};

float MYMATH_Pow(float a, uint8_t b)
{
	float result = 1;
	for(; b > 0; b--)
	{
		result *= a;
	}
	return result;
}
//正弦函数
float MYMATH_Sin(float ang)
{
	float value = 0;
	//确保0<=ang<=360;
	while(ang < 0)
		ang += 360;
	while(ang > 360)
		ang -= 360;
	
	if(ang <= 90)				value = argTab[(int)(ang * 2)];
	else if(ang <= 180)	value = argTab[(int)((180 - ang) * 2)];
	else if(ang <= 270)	value = -argTab[(int)((ang - 180) * 2)];
	else								value = -argTab[(int)((360 - ang) * 2)];
	
	return value;
}
//余弦函数
float MYMATH_Cos(float ang)
{
	return MYMATH_Sin(ang + 90);
}

//反正弦函数
float MYMATH_arcSin(float value)
{
	uint8_t i = 0;
	uint8_t sign = 0;
	
	if(value > 0)
	{
		sign = 1;
		if(value > 0.9999)
			return 90;
	}
	else if(value < 0)
	{
		sign = 0;
		value = -value;
		if(value < -0.9999)
			return -90;
	}
	else return 0;
	
	for(i = 0; i < 181; i++)
	{
		if(value <= argTab[i])
		{
			if(sign == 1) return (float)i/2;
			else 					return -(float)i/2;
		}
	}
	return 180;
}
//反余弦函数
float MYMATH_arcCos(float value)
{
	return 90-MYMATH_arcSin(value);
}

static const int16_t sin0_90_table[] = {
    0,     572,   1144,  1715,  2286,  2856,  3425,  3993,  4560,  5126,  5690,  6252,  6813,  7371,  7927,  8481,
    9032,  9580,  10126, 10668, 11207, 11743, 12275, 12803, 13328, 13848, 14364, 14876, 15383, 15886, 16383, 16876,
    17364, 17846, 18323, 18794, 19260, 19720, 20173, 20621, 21062, 21497, 21925, 22347, 22762, 23170, 23571, 23964,
    24351, 24730, 25101, 25465, 25821, 26169, 26509, 26841, 27165, 27481, 27788, 28087, 28377, 28659, 28932, 29196,
    29451, 29697, 29934, 30162, 30381, 30591, 30791, 30982, 31163, 31335, 31498, 31650, 31794, 31927, 32051, 32165,
    32269, 32364, 32448, 32523, 32587, 32642, 32687, 32722, 32747, 32762, 32767
};

int16_t lv_trigo_sin(int16_t angle)
{
    int16_t ret = 0;
    angle       = angle % 360;

    if(angle < 0) angle = 360 + angle;

    if(angle < 90) {
        ret = sin0_90_table[angle];
    }
    else if(angle >= 90 && angle < 180) {
        angle = 180 - angle;
        ret   = sin0_90_table[angle];
    }
    else if(angle >= 180 && angle < 270) {
        angle = angle - 180;
        ret   = -sin0_90_table[angle];
    }
    else {   /*angle >=270*/
        angle = 360 - angle;
        ret   = -sin0_90_table[angle];
    }

    return ret;
}

const float SIN[360] = {
	0.000000,0.017452,0.034899,0.052336,0.069756,0.087156,0.104528,0.121869,0.139173,0.156434,0.173648,0.190809,
	0.207912,0.224951,0.241922,0.258819,0.275637,0.292372,0.309017,0.325568,0.342020,0.358368,0.374607,0.390731,
	0.406737,0.422618,0.438371,0.453990,0.469472,0.484810,0.500000,0.515038,0.529919,0.544639,0.559193,0.573576,
	0.587785,0.601815,0.615661,0.629320,0.642788,0.656059,0.669131,0.681998,0.694658,0.707107,0.719340,0.731354,
	0.743145,0.754710,0.766044,0.777146,0.788011,0.798636,0.809017,0.819152,0.829038,0.838671,0.848048,0.857167,
	0.866025,0.874620,0.882948,0.891007,0.898794,0.906308,0.913545,0.920505,0.927184,0.933580,0.939693,0.945519,
	0.951057,0.956305,0.961262,0.965926,0.970296,0.974370,0.978148,0.981627,0.984808,0.987688,0.990268,0.992546,
	0.994522,0.996195,0.997564,0.998630,0.999391,0.999848,1.000000,0.999848,0.999391,0.998630,0.997564,0.996195,
	0.994522,0.992546,0.990268,0.987688,0.984808,0.981627,0.978148,0.974370,0.970296,0.965926,0.961262,0.956305,
	0.951057,0.945519,0.939693,0.933580,0.927184,0.920505,0.913545,0.906308,0.898794,0.891007,0.882948,0.874620,
	0.866025,0.857167,0.848048,0.838671,0.829038,0.819152,0.809017,0.798636,0.788011,0.777146,0.766044,0.754710,
	0.743145,0.731354,0.719340,0.707107,0.694658,0.681998,0.669131,0.656059,0.642788,0.629320,0.615661,0.601815,
	0.587785,0.573576,0.559193,0.544639,0.529919,0.515038,0.500000,0.484810,0.469472,0.453990,0.438371,0.422618,
	0.406737,0.390731,0.374607,0.358368,0.342020,0.325568,0.309017,0.292372,0.275637,0.258819,0.241922,0.224951,
	0.207912,0.190809,0.173648,0.156434,0.139173,0.121869,0.104528,0.087156,0.069756,0.052336,0.034899,0.017452,
	0.000000,-0.017452,-0.034899,-0.052336,-0.069756,-0.087156,-0.104528,-0.121869,-0.139173,-0.156434,-0.173648,-0.190809,
	-0.207912,-0.224951,-0.241922,-0.258819,-0.275637,-0.292372,-0.309017,-0.325568,-0.342020,-0.358368,-0.374607,-0.390731,
	-0.406737,-0.422618,-0.438371,-0.453990,-0.469472,-0.484810,-0.500000,-0.515038,-0.529919,-0.544639,-0.559193,-0.573576,
	-0.587785,-0.601815,-0.615661,-0.629320,-0.642788,-0.656059,-0.669131,-0.681998,-0.694658,-0.707107,-0.719340,-0.731354,
	-0.743145,-0.754710,-0.766044,-0.777146,-0.788011,-0.798636,-0.809017,-0.819152,-0.829038,-0.838671,-0.848048,-0.857167,
	-0.866025,-0.874620,-0.882948,-0.891007,-0.898794,-0.906308,-0.913545,-0.920505,-0.927184,-0.933580,-0.939693,-0.945519,
	-0.951057,-0.956305,-0.961262,-0.965926,-0.970296,-0.974370,-0.978148,-0.981627,-0.984808,-0.987688,-0.990268,-0.992546,
	-0.994522,-0.996195,-0.997565,-0.998631,-0.999391,-0.999848,-1.000000,-0.999848,-0.999391,-0.998631,-0.997565,-0.996195,
	-0.994522,-0.992546,-0.990268,-0.987688,-0.984808,-0.981627,-0.978148,-0.974370,-0.970296,-0.965926,-0.961262,-0.956305,
	-0.951057,-0.945519,-0.939693,-0.933580,-0.927184,-0.920505,-0.913545,-0.906308,-0.898794,-0.891007,-0.882948,-0.874620,
	-0.866025,-0.857167,-0.848048,-0.838671,-0.829038,-0.819152,-0.809017,-0.798636,-0.788011,-0.777146,-0.766044,-0.754710,
	-0.743145,-0.731354,-0.719340,-0.707107,-0.694658,-0.681998,-0.669131,-0.656059,-0.642788,-0.629320,-0.615661,-0.601815,
	-0.587785,-0.573576,-0.559193,-0.544639,-0.529919,-0.515038,-0.500000,-0.484810,-0.469472,-0.453990,-0.438371,-0.422618,
	-0.406737,-0.390731,-0.374607,-0.358368,-0.342020,-0.325568,-0.309017,-0.292372,-0.275637,-0.258819,-0.241922,-0.224951,
	-0.207912,-0.190809,-0.173648,-0.156434,-0.139173,-0.121869,-0.104528,-0.087156,-0.069756,-0.052336,-0.034899,-0.017452,
};

const float COS[360] = {
	1.000000,0.999848,0.999391,0.998630,0.997564,0.996195,0.994522,0.992546,0.990268,0.987688,0.984808,0.981627,
	0.978148,0.974370,0.970296,0.965926,0.961262,0.956305,0.951057,0.945519,0.939693,0.933580,0.927184,0.920505,
	0.913545,0.906308,0.898794,0.891007,0.882948,0.874620,0.866025,0.857167,0.848048,0.838671,0.829038,0.819152,
	0.809017,0.798636,0.788011,0.777146,0.766044,0.754710,0.743145,0.731354,0.719340,0.707107,0.694658,0.681998,
	0.669131,0.656059,0.642788,0.629320,0.615661,0.601815,0.587785,0.573576,0.559193,0.544639,0.529919,0.515038,
	0.500000,0.484810,0.469472,0.453990,0.438371,0.422618,0.406737,0.390731,0.374607,0.358368,0.342020,0.325568,
	0.309017,0.292372,0.275637,0.258819,0.241922,0.224951,0.207912,0.190809,0.173648,0.156434,0.139173,0.121869,
	0.104528,0.087156,0.069756,0.052336,0.034899,0.017452,0.000000,-0.017452,-0.034899,-0.052336,-0.069756,-0.087156,
	-0.104528,-0.121869,-0.139173,-0.156434,-0.173648,-0.190809,-0.207912,-0.224951,-0.241922,-0.258819,-0.275637,-0.292372,
	-0.309017,-0.325568,-0.342020,-0.358368,-0.374607,-0.390731,-0.406737,-0.422618,-0.438371,-0.453990,-0.469472,-0.484810,
	-0.500000,-0.515038,-0.529919,-0.544639,-0.559193,-0.573576,-0.587785,-0.601815,-0.615661,-0.629320,-0.642788,-0.656059,
	-0.669131,-0.681998,-0.694658,-0.707107,-0.719340,-0.731354,-0.743145,-0.754710,-0.766044,-0.777146,-0.788011,-0.798636,
	-0.809017,-0.819152,-0.829038,-0.838671,-0.848048,-0.857167,-0.866025,-0.874620,-0.882948,-0.891007,-0.898794,-0.906308,
	-0.913545,-0.920505,-0.927184,-0.933580,-0.939693,-0.945519,-0.951057,-0.956305,-0.961262,-0.965926,-0.970296,-0.974370,
	-0.978148,-0.981627,-0.984808,-0.987688,-0.990268,-0.992546,-0.994522,-0.996195,-0.997564,-0.998630,-0.999391,-0.999848,
	-1.000000,-0.999848,-0.999391,-0.998630,-0.997564,-0.996195,-0.994522,-0.992546,-0.990268,-0.987688,-0.984808,-0.981627,
	-0.978148,-0.974370,-0.970296,-0.965926,-0.961262,-0.956305,-0.951057,-0.945519,-0.939693,-0.933580,-0.927184,-0.920505,
	-0.913545,-0.906308,-0.898794,-0.891007,-0.882948,-0.874620,-0.866025,-0.857167,-0.848048,-0.838671,-0.829038,-0.819152,
	-0.809017,-0.798636,-0.788011,-0.777146,-0.766044,-0.754710,-0.743145,-0.731354,-0.719340,-0.707107,-0.694658,-0.681998,
	-0.669131,-0.656059,-0.642788,-0.629320,-0.615661,-0.601815,-0.587785,-0.573576,-0.559193,-0.544639,-0.529919,-0.515038,
	-0.500000,-0.484810,-0.469472,-0.453990,-0.438371,-0.422618,-0.406737,-0.390731,-0.374607,-0.358368,-0.342020,-0.325568,
	-0.309017,-0.292372,-0.275637,-0.258819,-0.241922,-0.224951,-0.207912,-0.190809,-0.173648,-0.156434,-0.139173,-0.121869,
	-0.104528,-0.087156,-0.069756,-0.052336,-0.034899,-0.017452,-0.000000,0.017452,0.034899,0.052336,0.069756,0.087156,
	0.104528,0.121869,0.139173,0.156434,0.173648,0.190809,0.207912,0.224951,0.241922,0.258819,0.275637,0.292372,
	0.309017,0.325568,0.342020,0.358368,0.374607,0.390731,0.406737,0.422618,0.438371,0.453990,0.469472,0.484810,
	0.500000,0.515038,0.529919,0.544639,0.559193,0.573576,0.587785,0.601815,0.615661,0.629320,0.642788,0.656059,
	0.669131,0.681998,0.694658,0.707107,0.719340,0.731354,0.743145,0.754710,0.766044,0.777146,0.788011,0.798636,
	0.809017,0.819152,0.829038,0.838671,0.848048,0.857167,0.866025,0.874620,0.882948,0.891007,0.898794,0.906308,
	0.913545,0.920505,0.927184,0.933580,0.939693,0.945519,0.951057,0.956305,0.961262,0.965926,0.970296,0.974370,
	0.978148,0.981627,0.984808,0.987688,0.990268,0.992546,0.994522,0.996195,0.997564,0.998630,0.999391,0.999848,
};
